# -*- encoding: UTF-8 -*-

import math
import thread
import threading
from naoqi import ALModule
from naoqi import ALBroker
from naoqi import ALProxy
import sys
import os
import time
#import locate1
import cv2
import numpy as np
#import otsu
#import getFruit
import vision_definitions
#import videoprocessing

IP = "192.168.1.103"
PORT = 9559
myBroker1 = ALBroker("myBroker",
                     "0.0.0.0",  # listen to anyone
                     0,  # find a free port and use it
                     IP,  # parent broker IP
                     PORT)
sonarproxy = ALProxy("ALSonar")
sonarproxy.subscribe("myApplication")
memorypxy = ALProxy("ALMemory")
alproxy = ALProxy("ALMotion")
cam = ALProxy("ALVideoDevice")
tts = ALProxy("ALTextToSpeech")
posture = ALProxy("ALRobotPosture")

mindis=0.12
nameId=None
speed_x=0.5
speed_y=0
speed_z=0
flag=1
sleeptime=0.4
initx=-1
maxstepx = 0.08
maxstepy = 0.1
maxsteptheta = 0.2
maxstepfrequency = 0.6
stepheight = 0.02
torsowx = 0.0
torsowy = 0.0
moveConfig = [["MaxStepX", maxstepx],
              ["MaxStepY", maxstepy],
              ["MaxStepTheta", maxsteptheta],
              ["MaxStepFrequency", maxstepfrequency],
              ["StepHeight", stepheight],
              ["TorsoWx", torsowx],
              ["TorsoWy", torsowy]]
class runthread(threading.Thread):
    def __init__(self, name):
        threading.Thread.__init__(self)
        self.name = name
    def run(self):
        while True:
            # global speed_x,speed_y,speed_z
            alproxy.move(speed_x, speed_y, speed_z,moveConfig)
            if (flag != 1):
                alproxy.stopMove()
                break
            time.sleep(0.4)
        cam.unsubscribe("python_GVM")

class picthread(threading.Thread):
    def __init__(self, name):
        threading.Thread.__init__(self)
        self.name = name
    def run(self):
        i=0
        #endtime=0
        #starttime=0
        while True:
            # global speed_x,speed_y,speed_z
            img = getimg()
            #endtime=time.time()
            #print "runtime_2=%f ms\n" % (endtime - starttime) * 1000
            #starttime=time.time()
            target = videoprocessing.processimg(img, i)
            i+=1
            if target!=None:
                v=target[1]+target[3]
                u=int(target[0]+target[2]/2.0)
                x,y=locate1.convert2Dto3D((v, u), IP, PORT)
                #print math.atan(y/x)*180/math.pi
                print x,y
            else:
                x=0.0
                y=0.0
            if (x<1.0):
                global flag
                flag=0
                break
            else:
                global speed_z
                #alproxy.stopMove()
                speed_z=-math.atan(y/x)/2.0

def sonardetect():
    ldis = memorypxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
    rdis = memorypxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
    sldis = str(ldis)
    srdis = str(rdis)
    sldis = sldis[0:4]
    srdis = srdis[0:4]

    sldis = float(sldis)
    srdis = float(srdis)

    LEFTDISTANCE = sldis
    RIGHTDISTANCE = srdis
    return (LEFTDISTANCE, RIGHTDISTANCE)

class sonarthread(threading.Thread):
    def __init__(self, name):
        threading.Thread.__init__(self)
        self.name = name

    def run(self):
        while True:
            global speed_x, leftdis, rightdis, fixangle, flag, sonarflag
            leftdis, rightdis = sonardetect()
            print "%f,%f" % (leftdis, rightdis)
            if (math.fabs(min(leftdis, rightdis) - mindis)<0.01):
                flag=0
                sonarproxy.unsubscribe("myApplication")
                break
            time.sleep(0.8)

run=runthread("A")
pthread=picthread("B")

def findballo():
    sonarproxy.subscribe("myApplication")
    while True:
        leftdis, rightdis = sonardetect()
        print "%f,%f" % (leftdis, rightdis)
        if (math.fabs(min(leftdis, rightdis) - mindis) < 0.02):
            break
        elif(min(leftdis, rightdis) > mindis):
            alproxy.moveTo(0.1,0.0,0.0)
        else:
            alproxy.moveTo(-0.1,0.0,0.0)
    #img = getimg()
    #target = videoprocessing.processimg(img, 100)
    #alproxy.moveTo(0,0.2)

def getimg():
    #starttime=time.time()
    img0 = cam.getImageRemote(nameId)
    #endtime=time.time()
    #print "runtime=%f ms\n" % (endtime - starttime) * 1000
    array = bytearray(img0[6])  # img[6]图像信息 Bytearray类型是一个可变的序列，
    # 并且序列中的元素的取值范围为 [0 ,255]
    img = np.array(array)  # 将array创建数组
    img = img.reshape([img0[1], img0[0], 3])  # 480*640 3个通道
    return img

def main():
    #starttime = time.time()
    cam.unsubscribe("python_GVM")
    for i in range(1, 8):
        temp = "python_GVM_%d" % i
        # print temp
        cam.unsubscribe(temp)
    cam.setActiveCamera(0)  # 0 is up camera
    # Register a Generic Video Module
    resolution = vision_definitions.kVGA
    colorSpace = vision_definitions.kBGRColorSpace
    fps = 30
    global nameId
    nameId = cam.subscribe("python_GVM", resolution, colorSpace, fps)
    # 设置曝光度模式
    cam.setCamerasParameter(nameId, 22, 2)
    pthread.start()
    #run.start()
    #endtime=time.time()
    #print "runtime_2=%f ms\n" % (endtime - starttime) * 1000
    #cv2.imwrite("./bottle_pic/bottle_pic.jpg",img)
    #min_color,max_color,red_threshold=otsu.train_parameter(img)
    # min_color=np.array([100,162,146])
    # max_color=np.array([109,225,195])
    # red_threshold=61
    # # 地下室
    # # [100, 162, 92][109,225,135]
    # # 35
    # # 实验室
    # # [100,162,146][109,227,195]
    # # 61
    # target=getFruit.countLocation(min_color,max_color,red_threshold,img)
    # print target
    # # up-left:(x=target[0],y=target[1]),wid(for x)=target[2],hei(for y)=target[3]
    # x,y=locate1.convert2Dto3D((target[1]+target[3],target[0]+target[2]/2),IP,PORT)
    # print x,y
    # print "distance=%f\n"%((x*x+y*y)**0.5)

if __name__ == '__main__':
    #main()
    findball()